#!/usr/bin/env python
# -*- coding: UTF-8 -*-
from device_manager.arm_manager import ArmManager
from device_manager.gripper_manager import GripperManager
import tf.transformations
import rospy
import numpy as np
from  geometry_msgs.msg import Pose
from  geometry_msgs.msg import Point

# class Demov2():
#     def __init__(self):
#         self.gm   = GripperManager()
#         self.arm  = ArmManager()
#         self.gm.activate()
#         rospy.sleep(0.1)
        
#         self.position = np.zeros((3,),dtype=float)
#         self.orientation = np.zeros((4,),dtype=float)

#         # 设置允许的最大速度和加速度，默认均为0.3
#         self.arm.set_max_acceleration_and_velocity(0.2,0.2)

#         self.count = 0

#     def __pose_callback(self, msgpose):

#         if msgpose.y>-0.2 and msgpose.x<-0.3 and msgpose.x>-0.6:
#             if self.count <5:
#                 self.count = self.count +1
#             # else:
#                 self.gm.open()
#                 rospy.sleep(0.1)
#                 or1 = tf.transformations.quaternion_from_euler(0,-1.5707,1.5707,axes='szyx')
#                 print msgpose
#                 position1 = np.zeros((3,),dtype=float)
#                 orientation1 = np.zeros((4,),dtype=float)
#                 position1[0] = msgpose.x
#                 position1[1] = msgpose.y
#                 position1[2] = msgpose.z+0.5255
#                 print position1
#                 # rospy.sleep(0.1)
#                 orientation1[0] = or1[0]
#                 orientation1[1] = or1[1]
#                 orientation1[2] = or1[2]
#                 orientation1[3] = or1[3]

#                 self.arm.set_target_pose(position=[msgpose.x+0.1,msgpose.y,msgpose.z+0.5255],orientation=orientation1)
#                 self.arm.set_target_pose(position=position1,orientation=orientation1)
#                 self.gm.close(speed=50)
#                 rospy.sleep(1.5)
#                 self.gm.open(speed=50)
#                 rospy.sleep(0.5)
#                 self.arm.set_target_pose(position=[msgpose.x+0.1,msgpose.y,msgpose.z+0.5255+0.01],orientation=orientation1)
#                 self.arm.set_target_pose(position=self.position,orientation=self.orientation)

#                 self.count=6
    
#     def run(self):
#         pose = self.arm.get_current_pose()
#         self.position[0] = pose.position.x
#         self.position[1] = pose.position.y
#         self.position[2] = pose.position.z 
#         self.orientation[0] = pose.orientation.x
#         self.orientation[1] = pose.orientation.y
#         self.orientation[2] = pose.orientation.z
#         self.orientation[3] = pose.orientation.w
#         rospy.Subscriber("basepoint", Point, self.__pose_callback, queue_size=1)
#         rospy.spin()

# if __name__ == '__main__':
#     # 初始化ros
#     rospy.init_node('DEVICE_MANAGER', anonymous=False)
#     app = Demov2()
#     app.run()        







class Demov2():
    def __init__(self):
        self.gm   = GripperManager()
        self.arm  = ArmManager()
        self.gm.activate()
        rospy.sleep(0.1)
        
        self.position = np.zeros((3,),dtype=float)
        self.orientation = np.zeros((4,),dtype=float)

        # 设置允许的最大速度和加速度，默认均为0.3
        self.arm.set_max_acceleration_and_velocity(0.25,0.25)

        self.count = 0

    def __pose_callback(self, msgpose):
        print self.arm.get_current_joint_values()

        if msgpose.position.y>-0.2 and msgpose.position.x<-0.3 and msgpose.position.x>-0.6 and msgpose.position.z>1:
        # if msgpose.y>-0.2 and msgpose.x<-0.3 and msgpose.x>-0.6:
            if self.count <4:
                rospy.sleep(0.5)
                self.count = self.count +1
            else:
                self.gm.open()
                rospy.sleep(0.1)

                print msgpose
                or1 = tf.transformations.quaternion_from_euler(0,-1.5707,1.5707,axes='szyx')


                position1 = np.zeros((3,),dtype=float)
                orientation1 = np.zeros((4,),dtype=float)
                orientation2 = [or1[0],or1[1],or1[2],or1[3]]

                position1[0] = msgpose.position.x+0.12
                position1[1] = msgpose.position.y-0.03
                position1[2] = msgpose.position.z
                print position1
                # rospy.sleep(0.1)
                orientation1[0] = msgpose.orientation.x
                orientation1[1] = msgpose.orientation.y
                orientation1[2] = msgpose.orientation.z
                orientation1[3] = msgpose.orientation.w

            #     # print tf.transformations.euler_from_quaternion(self.orientation,axes='szyx')
            #     # # 姿态控制测试，p：xyz，o：xyzw
            #     # # self.arm.back_to_zero()
        
            #     # # orientation = tf.transformations.quaternion_from_euler(1.5707,0,0,axes='szyx')
                self.arm.set_target_pose(position=[position1[0]+0.09,position1[1],position1[2]],orientation=orientation2)
                self.arm.set_target_pose(position=position1,orientation=orientation1)
                self.gm.close(speed=80)
                rospy.sleep(1.5)
                # self.gm.open()
                # rospy.sleep(2.5)
                self.arm.set_target_pose(position=[position1[0]+0.09,position1[1],position1[2]],orientation=orientation2)
                rospy.sleep(5)
                self.gm.open()
                self.count=0
                self.arm.set_target_pose(position=self.position,orientation=self.orientation)
                rospy.sleep(0.5)

        # rospy.sleep(1)
#############################
        # 二指爪控制测试
        # self.gm.activate()
        # self.gm.open()
        # rospy.loginfo('gripper opened.')
        # rospy.sleep(2)
        # self.gm.close()
        # rospy.loginfo('gripper closed.')
        # rospy.sleep(2)
        # self.gm.open(speed=100)
        # rospy.loginfo('gripper opened.')
        # rospy.sleep(2)
        # self.gm.close(speed=50)
        # rospy.loginfo('gripper closed.')
        # rospy.sleep(2)
        # self.gm.open(force=50)
        # rospy.loginfo('gripper opened.')
        # rospy.sleep(2)
        # self.gm.close(force=20)
        # rospy.loginfo('gripper closed.')
        # rospy.sleep(5)
    
    def run(self):
        #   order：zyx  xyzw
        # self.arm.back_to_zero()
        # e_q = tf.transformations.quaternion_from_euler(0,-1.5707,1.5707,axes='szyx')
        # print e_q
        # self.arm.set_target_pose(orientation=e_q)

        pose = self.arm.get_current_pose()
        # print pose
        self.position[0] = pose.position.x
        self.position[1] = pose.position.y
        self.position[2] = pose.position.z 
        self.orientation[0] = pose.orientation.x
        self.orientation[1] = pose.orientation.y
        self.orientation[2] = pose.orientation.z
        self.orientation[3] = pose.orientation.w
        
        # # self.arm.set_target_pose(position=self.position)

        # # rospy.Subscriber("grasp_detection/predict_pose", Pose, self.__pose_callback, queue_size=1)
        rospy.Subscriber("grasp_detection/predict_pose", Pose, self.__pose_callback, queue_size=1)
        rospy.spin()
# set_joint_values
if __name__ == '__main__':
    # 初始化ros
    rospy.init_node('DEVICE_MANAGER', anonymous=False)
    app = Demov2()
    app.run()        



 



   

